#include "LensControl.h"
#include <QDebug>
#include <QEventLoop>
#include <QTimer>

LensControl::LensControl() :
	serialPort(nullptr)
{
}

LensControl::~LensControl()
{
	close();
}

bool LensControl::open(const QString& portname)
{
	serialPort = new QSerialPort(portname);
	bool ok = serialPort->open(QIODevice::ReadWrite);
	if (!ok) {
		delete serialPort;
		return false;
	}

	serialPort->setBaudRate(QSerialPort::Baud115200);
	serialPort->setDataBits(QSerialPort::Data8);
	serialPort->setParity(QSerialPort::NoParity);
	serialPort->setStopBits(QSerialPort::OneStop);
	serialPort->setFlowControl(QSerialPort::NoFlowControl);

	return true;
}

void LensControl::close()
{
	if (serialPort == nullptr) {
		return;
	}

	if (serialPort->isOpen()) {
		serialPort->close();
	}
	delete serialPort;
}

bool LensControl::isOpen()
{
	return serialPort != nullptr && serialPort->isOpen();
}

QString LensControl::version()
{
	sendCode({ version_code });
	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);
	if (!ret || retCodes.size() != 3) {
		return "no version";
	}

	return QString::number(retCodes[0]) + "." +
		QString::number(retCodes[1]) + "." +
		QString::number(retCodes[2]);
}

bool LensControl::systemInit(uchar& lensID, ushort& curFocus, QVector<ushort>& focusRange, ushort& maxFocus, ushort& curAperture, QVector<ushort>& apertureRange)
{
	sendCode({ init_code });
	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes, 100);
	if (!ret || retCodes.size() != 16 || retCodes[0] != 0) {
		return false;
	}

	focusRange.resize(2);
	apertureRange.resize(2);
	this->focusRange.resize(2);
	this->apertureRange.resize(2);

	this->lensID = lensID = retCodes[1];
	currentFocus = curFocus = retCodes[2] << 8 | retCodes[3];
	this->focusRange[0] = focusRange[0] = retCodes[4] << 8 | retCodes[5];
	this->focusRange[1] = focusRange[1] = retCodes[6] << 8 | retCodes[7];
	this->maxFocus = maxFocus = retCodes[8] << 8 | retCodes[9];
	currentAperture = curAperture = retCodes[10] << 8 | retCodes[11];
	this->apertureRange[0] = apertureRange[0] = retCodes[12] << 8 | retCodes[13];
	this->apertureRange[1] = apertureRange[1] = retCodes[14] << 8 | retCodes[15];

	return true;
}

bool LensControl::setFocusPos(ushort value)
{
	if (value > maxFocus) {
		value = maxFocus;
	}

	const uchar focus_high = (value >> 8) & 0xff;
	const uchar focus_low = value & 0xff;
	sendCode({ set_focus_code, focus_high, focus_low });

	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);

	if (!ret || retCodes.size() != 1 || retCodes[0] != 0) {
		return false;
	}

	return ret;
}

bool LensControl::setAperturePos(ushort value)
{
	if (value < apertureRange[0]) {
		value = apertureRange[0];
	} else if (value > apertureRange[1]) {
		value = apertureRange[1];
	}

	const uchar aper_high = (value >> 8) & 0xff;
	const uchar aper_low = value & 0xff;
	sendCode({ set_aperture_code, aper_high, aper_low });

	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);

	if (!ret || retCodes.size() != 1 || retCodes[0] != 0) {
		return false;
	}

	return ret;
}

int LensControl::getFocusPos()
{
	sendCode({ get_focus_pos_code });
	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);

	if (!ret || retCodes.size() != 3 || retCodes[0] != 0) {
		return -1;
	}

	return retCodes[1] << 8 | retCodes[2];
}

int LensControl::getAperture(QVector<ushort>& apertureRange)
{
	sendCode({ get_aperture_code});
	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);

	if (!ret || retCodes.size() != 7 || retCodes[0] != 0) {
		return -1;
	}

	this->apertureRange.resize(2);
	apertureRange.resize(2);

	this->apertureRange[0] = apertureRange[0] = retCodes[3] << 8 | retCodes[4];
	this->apertureRange[1] = apertureRange[1] = retCodes[5] << 8 | retCodes[6];

	return retCodes[1] << 8 | retCodes[2];
}

int LensControl::getFocus()
{
	sendCode({ get_focus_range_code });
	QVector<uchar> retCodes;
	bool ret = recvCode(retCodes);

	if (!ret || retCodes.size() != 3 || retCodes[0] != 0) {
		return -1;
	}

	return retCodes[1] << 8 | retCodes[2];
}

void LensControl::sendCode(const QVector<uchar>& codes)
{
	if (serialPort == nullptr || !serialPort->isOpen()) {
		return;
	}
	
	QVector<uchar> extendCodes = codes;
	extendCodes.resize(5);

	QByteArray bytes(11, 0x00);
	bytes[0] = 0x55;
	bytes[1] = 0xaa;
	bytes[2] = 0x07;
	bytes[3] = 0x01;
	for (int i = 4; i < 4 + extendCodes.size(); ++i) {
		bytes[i] = extendCodes[i - 4];
	}
	bytes[9] = verifyCode(QVector<uchar>(bytes.begin() + 2, bytes.begin() + 9));
	bytes[10] = 0xf0;

	serialPort->write(bytes);
	serialPort->flush();

	// qDebug() << "send: " << bytes.toHex();
}

bool LensControl::recvCode(QVector<uchar>& codes, int msec)
{
	if (serialPort == nullptr || !serialPort->isOpen()) {
		return false;
	}

	QEventLoop loop;
	QTimer timer;
	timer.setInterval(30000);
	QObject::connect(serialPort, &QSerialPort::readyRead, &loop, &QEventLoop::quit);
	QObject::connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit);

	loop.exec();

	if (timer.isSingleShot()) {
		return false;
	}
	timer.stop();

	serialPort->waitForReadyRead(msec);
	QByteArray bytes = serialPort->readAll();
	bytes = convert(bytes);
	// qDebug() << "recv: " << bytes.toHex();
	uchar len = bytes[2];

	if (len + 4 != bytes.size()) {
		serialPort->readAll();
		return false;
	}

	const uchar verify = verifyCode(QVector<uchar>(bytes.begin() + 2, bytes.begin() + 2 + len));
	if (bytes.at(bytes.size() - 2) != verify) {
		return false;
	}

	codes = QVector<uchar>(bytes.begin() + 5, bytes.begin() + 2 + len);
	return true;
}

uchar LensControl::verifyCode(const QVector<uchar>& codes)
{
	if (codes.empty()) {
		return 0;
	}

	uchar ret = codes[0];
	for (int i = 1; i < codes.size(); ++i) {
		ret ^= codes[i];
	}

	return ret;
}

QByteArray LensControl::convert(QByteArray& bytes)
{
	QStringList codes = QString(bytes).split(" ");

	// pop \r\n
	codes.pop_back();

	QByteArray new_bytes;
	for (auto& code: codes) {
		if (code.isEmpty()) {
			continue;
		}
		new_bytes.push_back(code.toUInt(nullptr, 16));
	}

	return new_bytes;
}


